//---------------------------------------------------------------------------

#include <vcl.h>
#pragma hdrstop

#include "SimComp.h"
#include "Configurator.h"
#include "GlobalVariables.h"
//---------------------------------------------------------------------------
#pragma package(smart_init)
#pragma resource "*.dfm"
TFormSimComp *FormSimComp;

unsigned char C2B_data[C2B_LENGTH] = {0};
unsigned char B2C_Adult_data[B2C_ADULT_LENGTH] = {0};
unsigned char B2C_Kid_data[B2C_KID_LENGTH] = {0};
unsigned char MotionQueue[MOTION_QUEUE_SIZE] = {0};
float DisQueue[MOTION_QUEUE_SIZE] = {0};
float AngleQueue[MOTION_QUEUE_SIZE] = {0};
int mindex_head = 0;
int mindex_tail = 0;
int mindex_run = 0;
bool RunMotionStrategy = false;
unsigned char RunDIP = 0;
bool InitialRun = true;
bool MotionDone = false;

//---------------------------------------------------------------------------
__fastcall TFormSimComp::TFormSimComp(TComponent* Owner)
	: TForm(Owner)
{
}
//---------------------------------------------------------------------------

bool TFormSimComp::B2C_DataCheck(unsigned char* buf, int length_buf)
{
	int num_nondealed_data = 0;
	for(int i=0; i < length_buf; i++)
	{
		RXQueue[RX_tail] = buf[i];
		RX_tail = (RX_tail + 1) % RXQUEUE_SIZE;
	}
	if (RX_head <= RX_tail) {
		num_nondealed_data = RX_tail - RX_head;
	}
	else {
		num_nondealed_data = RXQUEUE_SIZE - (RX_head - RX_tail);
	}

	if(RadioGroupRobotType->ItemIndex == ADULT_SIZED)
	{
		while(num_nondealed_data >= B2C_ADULT_LENGTH)
		{
			if(RXQueue[((RX_head+B2C_ADULT_HEAD1) % RXQUEUE_SIZE)] == 255 &&
				 RXQueue[((RX_head+B2C_ADULT_HEAD2) % RXQUEUE_SIZE)] == 254 )
			{
				unsigned char checksum = 0;
				for(int i=B2C_ADULT_DIP; i<B2C_ADULT_CHECKSUM; i++)
				{
					checksum += RXQueue[((RX_head+i) % RXQUEUE_SIZE)];
				}
				checksum = ~checksum;
				if(checksum == RXQueue[((RX_head+B2C_ADULT_CHECKSUM) % RXQUEUE_SIZE)])
				{
					for(int i=B2C_ADULT_HEAD1; i<B2C_ADULT_LENGTH; i++)
					{
						B2C_Adult_data[i] = RXQueue[((RX_head+i) % RXQUEUE_SIZE)];
					}
					UpdateRecInfo(ADULT_SIZED);

					if(RunMotionStrategy)
					{
						RunMotionQueue();
					}
					RX_head = 0;
					RX_tail = 0;
					return true;
				}
				else
				{
					RX_head = (RX_head + 1) % RXQUEUE_SIZE;
				}
			}
			else
			{
				RX_head = (RX_head + 1) % RXQUEUE_SIZE;
			}

			if (RX_head <= RX_tail) {
				num_nondealed_data = RX_tail - RX_head;
			}
			else {
				num_nondealed_data = RXQUEUE_SIZE - (RX_head - RX_tail);
			}
		}
	}
	else if(RadioGroupRobotType->ItemIndex == KID_SIZED)
	{
		while(num_nondealed_data >= B2C_KID_LENGTH)
		{
			if(RXQueue[((RX_head+B2C_KID_HEAD1) % RXQUEUE_SIZE)] == 255 &&
				 RXQueue[((RX_head+B2C_KID_HEAD2) % RXQUEUE_SIZE)] == 255 )
			{
				unsigned char checksum = 0;
				for(int i=B2C_KID_DIP; i<B2C_KID_CHECKSUM; i++)
				{
					checksum += RXQueue[((RX_head+i) % RXQUEUE_SIZE)];
				}
				checksum = ~checksum;
				if(checksum == RXQueue[((RX_head+B2C_KID_CHECKSUM) % RXQUEUE_SIZE)])
				{
					for(int i=B2C_KID_HEAD1; i<B2C_KID_LENGTH; i++)
					{
						B2C_Kid_data[i] = RXQueue[((RX_head+i) % RXQUEUE_SIZE)];
					}
					UpdateRecInfo(KID_SIZED);

					if(RunMotionStrategy)
					{
						RunMotionQueue();
					}
					RX_head = 0;
					RX_tail = 0;
					return true;
				}
				else
				{
					RX_head = (RX_head + 1) % RXQUEUE_SIZE;
				}
			}
			else
			{
				RX_head = (RX_head + 1) % RXQUEUE_SIZE;
			}

			if (RX_head <= RX_tail) {
				num_nondealed_data = RX_tail - RX_head;
			}
			else {
				num_nondealed_data = RXQUEUE_SIZE - (RX_head - RX_tail);
			}
		}
  }
	return false;
}
//---------------------------------------------------------------------------

void TFormSimComp::UpdateRecInfo(int type)
{
	if(type == ADULT_SIZED)
	{
		DIP_mode = B2C_Adult_data[B2C_ADULT_DIP];
		RHeadPitch = B2C_Adult_data[B2C_ADULT_HEAD_PITCH_L] + (B2C_Adult_data[B2C_ADULT_HEAD_PITCH_H]<<8);
		RHeadYaw = B2C_Adult_data[B2C_ADULT_HEAD_YAW_L] + (B2C_Adult_data[B2C_ADULT_HEAD_YAW_H]<<8);
		RWaistRoll = B2C_Adult_data[B2C_ADULT_WAIST_ROLL_L] + (B2C_Adult_data[B2C_ADULT_WAIST_ROLL_H]<<8);
		RWaistPitch = B2C_Adult_data[B2C_ADULT_WAIST_PITCH_L] + (B2C_Adult_data[B2C_ADULT_WAIST_PITCH_H]<<8);
		RWaistYaw = B2C_Adult_data[B2C_ADULT_WAIST_YAW_L] + (B2C_Adult_data[B2C_ADULT_WAIST_YAW_H]<<8);
		MotionDoing = B2C_Adult_data[B2C_ADULT_MOTION_DONE];
		Azimuth = B2C_Adult_data[B2C_ADULT_AZIMUTH_L] + (B2C_Adult_data[B2C_ADULT_AZIMUTH_H]<<8);
	}
	else if(type == KID_SIZED)
	{
		DIP_mode = B2C_Kid_data[B2C_KID_DIP];
		RHeadPitch = B2C_Kid_data[B2C_KID_HEAD_PITCH_L] + (B2C_Kid_data[B2C_KID_HEAD_PITCH_H]<<8);
		RHeadYaw = B2C_Kid_data[B2C_KID_HEAD_YAW_L] + (B2C_Kid_data[B2C_KID_HEAD_YAW_H]<<8);
		RWaistRoll = 0;
		RWaistPitch = 0;
		RWaistYaw = 512;
		MotionDoing = B2C_Kid_data[B2C_KID_MOTION_DONE];
		Azimuth = B2C_Kid_data[B2C_KID_AZIMUTH_L] + (B2C_Kid_data[B2C_KID_AZIMUTH_H]*100);
  }

	Label_DIP->Caption = IntToStr(DIP_mode);
	Label_HP->Caption = IntToStr(RHeadPitch);
	Label_HY->Caption = IntToStr(RHeadYaw);
	Label_WR->Caption = IntToStr(RWaistRoll);
	Label_WP->Caption = IntToStr(RWaistPitch);
	Label_WY->Caption = IntToStr(RWaistYaw);
	Label_MD->Caption = IntToStr(MotionDoing);
	Label_Azimuth->Caption = IntToStr(Azimuth);
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::Edit_HPKeyPress(TObject *Sender, wchar_t &Key)
{
	if(Key==13)
	{
		ScrollBar_HP->Position = StrToInt(Edit_HP->Text);
	}
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::Edit_HYKeyPress(TObject *Sender, wchar_t &Key)
{
	if(Key==13)
	{
		ScrollBar_HY->Position = StrToInt(Edit_HY->Text);
  }
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::FormCreate(TObject *Sender)
{
	FormSimComp->Top = 150;
	FormSimComp->Left = 100;
	ScrollBar_HP->Position = 512;
	ScrollBar_HY->Position = 512;
	InitSendInfo();
	Button_MClearClick(NULL);
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::ScrollBar_HPChange(TObject *Sender)
{
	Edit_HP->Text = IntToStr(ScrollBar_HP->Position);
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::ScrollBar_HYChange(TObject *Sender)
{
	Edit_HY->Text = IntToStr(ScrollBar_HY->Position);
}
//---------------------------------------------------------------------------

void TFormSimComp::InitSendInfo(void)
{
	Edit_MD->Text = IntToStr(MotionMode);
	Edit_CD->Text = FormatFloat("0.0000",ContDis);
	Edit_CA->Text = FormatFloat("0.0000",ContAngle);
	Edit_HM->Text = IntToStr(HeadMode);
	Edit_HP->Text = IntToStr(SHeadPitch);
	Edit_HY->Text = IntToStr(SHeadYaw);
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::ButtonSendClick(TObject *Sender)
{
  Send_C2B_Data();
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::CheckBoxSendClick(TObject *Sender)
{
	if(CheckBoxSend->Checked)
	{
		FormMain->CheckBoxZigbeeSend->Checked = false;
		FormMain->CheckBoxCompSend->Checked = false;
		FormMain->CheckBoxFreeSend->Checked = false;
		FormMain->CheckBoxTXTSend->Checked = false;
	}
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::MotionButtonClick(TObject *Sender)
{
	unsigned char motion_mode = 0;
//	float distance = StrToFloat(Edit_MCD->Text);
//	float angle = StrToFloat(Edit_MCA->Text);
	float distance = 0;
	float angle = 0;
	unsigned char type = RadioGroup_Motiontype->ItemIndex;

	if(Sender == Button_WalkF)
	{
		motion_mode = (type==0)*11 + (type==1)*105 + (type==2)*62 + (type==3)*63;
		distance = StrToFloat(Edit_MCD->Text);
		angle = StrToFloat(Edit_MCA->Text);
		distance = (type==1)*distance + (type!=1)*0;
		angle = (type==1)*angle + (type!=1)*0;
	}
	else if(Sender == Button_WalkFF)
	{
    motion_mode = 13;
	}
	else if(Sender == Button_WalkB)
	{
		motion_mode = (type==0)*12 + (type==1)*105;
		distance = StrToFloat(Edit_MCD->Text);
		angle = StrToFloat(Edit_MCA->Text);
		distance = (type==1)*distance + (type!=1)*0;
		angle = (type==1)*angle + (type!=1)*0;
	}
	else if(Sender == Button_WalkBB)
	{
    motion_mode = 14;
	}
	else if(Sender == Button_ShiftR)
	{
		motion_mode = 17;
	}
	else if(Sender == Button_ShiftRR)
	{
		motion_mode = 19;
	}
	else if(Sender == Button_ShiftL)
	{
    motion_mode = 18;
	}
	else if(Sender == Button_ShiftLL)
	{
    motion_mode = 20;
	}
	else if(Sender == Button_TurnR)
	{
    motion_mode = 23;
	}
	else if(Sender == Button_TurnRR)
	{
		motion_mode = 25;
	}
	else if(Sender == Button_TurnL)
	{
		motion_mode = 24;
	}
	else if(Sender == Button_TurnLL)
	{
    motion_mode = 26;
	}
	else if(Sender == Button_TurnballR)
	{
		motion_mode = 29;
	}
	else if(Sender == Button_TurnballRR)
	{
		motion_mode = 0;
	}
	else if(Sender == Button_TurnballL)
	{
		motion_mode = 30;
	}
	else if(Sender == Button_TurnballLL)
	{
		motion_mode = 0;
	}
	else if(Sender == Button_KickR)
	{
		motion_mode = 31;
	}
	else if(Sender == Button_KickRR)
	{
		motion_mode = 33;
	}
	else if(Sender == Button_KickL)
	{
		motion_mode = 32;
	}
	else if(Sender == Button_KickLL)
	{
		motion_mode = 34;
	}
	else if(Sender == Button_SidekickR)
	{
		motion_mode = 45;
	}
	else if(Sender == Button_SidekickL)
	{
		motion_mode = 46;
	}
	else if(Sender == Button_Pick)
	{
		motion_mode = 74;
	}
	else if(Sender == Button_Throw)
	{
		motion_mode = 75;
		distance = StrToFloat(Edit_MCD->Text);
		angle = StrToFloat(Edit_MCA->Text);
	}
	else if(Sender == Button_LiftL)
	{
		motion_mode = 60;
	}
	else if(Sender == Button_LiftH)
	{
		motion_mode = 61;
	}
	else if(Sender == Button_Waist)
	{
		motion_mode = 77;
		distance = StrToFloat(Edit_MCD->Text);
		angle = StrToFloat(Edit_MCA->Text);
	}
	else if(Sender == Button_Stop)
	{
		motion_mode = 100;
	}
	else if(Sender == Button_Whatever)
	{
		motion_mode = StrToInt(Edit_MotionWhatever->Text);
		Edit_MotionWhatever->Text = IntToStr(motion_mode);
		distance = StrToFloat(Edit_MCD->Text);
		angle = StrToFloat(Edit_MCA->Text);
	}
	MemoMotion->Lines->Add(IntToStr(motion_mode));

	for(int i=0; i < StrToInt(Edit_Mcount->Text); i++)
	{
		MotionQueue[mindex_tail] = motion_mode;
		DisQueue[mindex_tail] = distance;
		AngleQueue[mindex_tail] = angle;
		mindex_tail = (mindex_tail+1) % MOTION_QUEUE_SIZE;
	}
	Edit_Mcount->Text = "1";
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::Button_MClearClick(TObject *Sender)
{
	for(int i=0; i<MOTION_QUEUE_SIZE; i++)
	{
		MotionQueue[i] = 0;
		DisQueue[i] = 0;
		AngleQueue[i] = 0;
  }

	mindex_head = 0;
	mindex_tail = 0;

	MemoMotion->Text = "";
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::Button_MshowClick(TObject *Sender)
{
	int temp_head = mindex_head;
	int temp_tail = mindex_tail;
	int count = 0;
	AnsiString mstr = "";

	while(temp_head != temp_tail)
	{
		mstr = IntToStr(count) + ": " +
					 IntToStr(MotionQueue[temp_head]) + ", D:" +
					 FormatFloat("0.00", DisQueue[temp_head]) + ", A:" +
					 FormatFloat("0.00", AngleQueue[temp_head]) ;
		temp_head = (temp_head+1) % MOTION_QUEUE_SIZE;
		MemoMotion->Lines->Add(mstr);
		count++;
	}
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::MemoMotionDblClick(TObject *Sender)
{
	static bool changesize = true;
	if(changesize)
	{
		MemoMotion->Height = 300;
	}
	else
	{
		MemoMotion->Height = 125;
	}
	changesize = !changesize;
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::MemoMotionMouseDown(TObject *Sender, TMouseButton Button,
          TShiftState Shift, int X, int Y)
{
	if(Button == mbMiddle)
	{
    MemoMotion->Text = "";
  }
}
//---------------------------------------------------------------------------

int TFormSimComp::RunMotionQueue(void)
{
	if(DIP_mode == RunDIP)
	{
		if(InitialRun)
		{
			mindex_run = mindex_head;
			Label_MHead->Caption = IntToStr(mindex_head);
			Label_MTail->Caption = IntToStr(mindex_tail);
			Label_MNow->Caption = IntToStr(mindex_run);

			Edit_MD->Text = IntToStr(MotionQueue[mindex_run]);
			Edit_CD->Text = FormatFloat("0.00", DisQueue[mindex_run]);
			Edit_CA->Text = FormatFloat("0.00", AngleQueue[mindex_run]);
			Edit_HM->Text = "0";
			Edit_HP->Text = "512";
			Edit_HY->Text = "512";
			Send_C2B_Data();
			InitialRun = false;
			MotionDone = false;
			return mindex_run;
		}

		// Check the motion board has received the motion mode and is performing the motion.
		if(MotionDoing == MotionQueue[mindex_run] && MotionDone==false)
		{
			MotionDone = true;
			return mindex_run;
		}
		// Check the motion is done
		if(((MotionDoing == 0) || (MotionDoing == 100)) && MotionDone)
		{
			if(mindex_run == mindex_tail)
			{
				return mindex_run;
			}
			mindex_run = (mindex_run+1) % MOTION_QUEUE_SIZE;
			MotionDone = false;
			if(mindex_run == mindex_tail)
			{
				return mindex_run;
			}
		}
		else if(MotionDone) // If the motion is not done, wait for the done signal.
		{
      return mindex_run;
		}

		Label_MNow->Caption = IntToStr(mindex_run);
		Edit_MD->Text = IntToStr(MotionQueue[mindex_run]);
		Edit_CD->Text = FormatFloat("0.00", DisQueue[mindex_run]);
		Edit_CA->Text = FormatFloat("0.00", AngleQueue[mindex_run]);
		Edit_HM->Text = "0";
		Edit_HP->Text = "512";
		Edit_HY->Text = "512";
		Send_C2B_Data();
	}
	else
	{
    InitialRun = true;
	}
	return mindex_run;
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::Button_RunMotionClick(TObject *Sender)
{
	RunMotionStrategy = true;
	RunDIP = StrToInt(Edit_RunDIP->Text) % 100;
	Edit_RunDIP->Text = IntToStr(RunDIP);
	Edit_RunDIP->Enabled = false;

	Edit_MD->Enabled = false;
	Edit_CD->Enabled = false;
	Edit_CA->Enabled = false;
	Edit_HM->Enabled = false;
	Edit_HP->Enabled = false;
	Edit_HY->Enabled = false;
	ButtonSend->Enabled = false;
	CheckBoxSend->Enabled = false;
	GroupBoxSendInfo->Enabled = false;
	FormMain->ButtonClearSendClick(NULL);

	if(FormMain->Timer_ContSend->Enabled)
	{
    FormMain->Button_ContTimerEnableClick(NULL);
  }
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::Button_StopMotionClick(TObject *Sender)
{
	RunMotionStrategy = false;
	Edit_RunDIP->Enabled = true;

	Edit_MD->Enabled = true;
	Edit_CD->Enabled = true;
	Edit_CA->Enabled = true;
	Edit_HM->Enabled = true;
	Edit_HP->Enabled = true;
	Edit_HY->Enabled = true;
	ButtonSend->Enabled = true;
	CheckBoxSend->Enabled = true;
//	GroupBoxSendInfo->Enabled = true;
}
//---------------------------------------------------------------------------

void TFormSimComp::Send_C2B_Data(void)
{
	unsigned char checksum = 0;
	AnsiString str_send = "";
	MotionMode = StrToInt(Edit_MD->Text);
	ContDis = StrToFloat(Edit_CD->Text);
	ContAngle = StrToFloat(Edit_CA->Text);
	HeadMode = StrToInt(Edit_HM->Text);
	SHeadPitch = StrToInt(Edit_HP->Text);
	SHeadYaw = StrToInt(Edit_HY->Text);

	C2B_data[C2B_HEAD1] = 0xFF;
	C2B_data[C2B_MOTION_MODE] = MotionMode;
	C2B_data[C2B_CONT_DIS_L] = ((int)(ContDis*100 + 32768)) & 0x00FF;
	C2B_data[C2B_CONT_DIS_H] = ((int)(ContDis*100 + 32768)) >> 8;
	C2B_data[C2B_CONT_ANGLE_L] = ((int)(ContAngle*100 + 32768)) & 0x00FF;
	C2B_data[C2B_CONT_ANGLE_H] = ((int)(ContAngle*100 + 32768)) >> 8;
	C2B_data[C2B_HEAD_MODE] = HeadMode;
	C2B_data[C2B_MOTOR_0_L] = SHeadPitch & 0x00FF;
	C2B_data[C2B_MOTOR_0_H] = (SHeadPitch & 0xFF00) >> 8;
	C2B_data[C2B_MOTOR_1_L] = SHeadYaw & 0x00FF;
	C2B_data[C2B_MOTOR_1_H] = (SHeadYaw & 0xFF00) >> 8;

	str_send = ">> " + IntToStr(C2B_data[C2B_HEAD1]);
	for(int i=C2B_HEAD1+1; i<C2B_CHECKSUM; i++)
	{
    str_send = str_send + ", " + IntToStr(C2B_data[i]);
		checksum += C2B_data[i];
	}
	checksum = ~checksum;

	C2B_data[C2B_CHECKSUM] = checksum;
	str_send = str_send + ", " + IntToStr(C2B_data[C2B_CHECKSUM]);

	FormMain->MemoSend->Lines->Add(str_send);

	if(FormMain->YbComm1->Active)
	{
		FormMain->MemoSend->Lines->Add("Send");
		FormMain->YbComm1->OutQueue->Clear();
		FormMain->YbComm1->Write(C2B_data, C2B_LENGTH);
	}
}
void __fastcall TFormSimComp::Button_DefaultClick(TObject *Sender)
{
	Button_MClearClick(NULL);
	MotionButtonClick(Button_LiftL);
	MotionButtonClick(Button_WalkF);
	MotionButtonClick(Button_WalkF);
	MotionButtonClick(Button_WalkF);
	MotionButtonClick(Button_TurnL);
	MotionButtonClick(Button_TurnL);
	MotionButtonClick(Button_LiftH);
	MotionButtonClick(Button_WalkF);
	MotionButtonClick(Button_WalkF);
	MotionButtonClick(Button_WalkF);
	MotionButtonClick(Button_WalkF);
	MotionButtonClick(Button_WalkF);
	MotionButtonClick(Button_WalkF);
	Button_MshowClick(NULL);
}
//---------------------------------------------------------------------------

void __fastcall TFormSimComp::Button_MshowMouseDown(TObject *Sender, TMouseButton Button,
          TShiftState Shift, int X, int Y)
{
	if(Button == mbRight)
	{
		Button_Default->Enabled = !Button_Default->Enabled;
  }
}
//---------------------------------------------------------------------------

